#include "vec.h"
#include <math.h>
static float mysqrt(float v)
{
    return sqrtf(v);
}
/****************************** vec2 ******************************************/
vec2* vec2_copy(vec2* des, const vec2* src)
{
    des->x = src->x;
    des->y = src->y;
    return des;
}
vec2* vec2_add_copy(vec2* out, const vec2* left, const vec2* right)
{
    out->x = left->x + right->x;
    out->y = left->y + right->y;
    return out;
}
vec2* vec2_add(vec2* left, const vec2* right)
{
    left->x += right->x;
    left->y += right->y;
    return left;
}
float vec2_dot(const vec2* left, const vec2* right)
{
    vec2 out;
    vec2_mult_v2_copy(&out, left, right);
    return out.x + out.y;
}

vec2* vec2_mult_v2_copy(vec2* out, const vec2* left, const vec2* right)
{
    out->x = left->x * right->x;
    out->y = left->y * right->y;
    return out;
}
vec2* vec2_mult_v2(vec2* left, const vec2* right)
{
    vec2 out;
    vec2_mult_v2_copy(&out, left, right);
    return vec2_copy(left, &out);
}
vec2* vec2_mult_v_copy(vec2* out, const vec2* left, float v)
{
    out->x = left->x * v;
    out->y = left->y * v;
    return out;
}
vec2* vec2_mult_v(vec2* left, float v)
{
    vec2 out;
    vec2_mult_v_copy(&out, left, v);
    return vec2_copy(left, &out);
}
vec2* vec2_normalize_copy(vec2* out, const vec2* v2)
{
    float inversesqrt = 1 / (float)sqrt(vec2_dot(v2, v2));
    return vec2_mult_v_copy(out, v2, inversesqrt);
}
vec2* vec2_normalize(vec2* v2)
{
    vec2 out;
    vec2_normalize_copy(&out, v2);
    return vec2_copy(v2, &out);
}
vec2* vec2_set(vec2* out, float x, float y)
{
    out->x = x;
    out->y = y;
    return out;
}
float vec2_length(const vec2* v2)
{
    return mysqrt(vec2_dot(v2, v2));
}
/****************************** vec3 ******************************************/
vec3* vec3_copy(vec3* des, const vec3* src)
{
    des->x = src->x;
    des->y = src->y;
    des->z = src->z;
    return des;
}
vec3* vec3_set(vec3* out, float x, float y, float z)
{
    out->x = x;
    out->y = y;
    out->z = z;
    return out;
}
vec3* vec3_add_copy(vec3* out, const vec3* left, const vec3* right)
{
    out->x = left->x + right->x;
    out->y = left->y + right->y;
    out->z = left->z + right->z;
    return out;
}
vec3* vec3_add(vec3* left, const vec3* right)
{
   vec3 out;
   vec3_add_copy(&out, left, right);
   return vec3_copy(left,&out);
}

vec3* vec3_mult_v3_copy(vec3* out, const vec3* left, const vec3* right)
{
    out->x = left->x * right->x;
    out->y = left->y * right->y;
    out->z = left->z * right->z;
    return out;
}
vec3* vec3_mult_v3(vec3* left, const vec3* right)
{
    vec3 out;
    vec3_mult_v3_copy(&out, left, right);
    return vec3_copy(left, &out);
}

vec3* vec3_mult_v_copy(vec3* out, const vec3* left, float v)
{
    out->x = left->x * v;
    out->y = left->y * v;
    out->z = left->z * v;
    return out;
}
vec3* vec3_mult_v(vec3* left, float v)
{
    vec3 out;
    vec3_mult_v_copy(&out, left, v);
    return vec3_copy(left, &out);
}

vec3* vec3_cross_copy(vec3* out, const vec3* left, const vec3* right)
{
    out->x = left->y * right->z - right->y * left->z;
    out->y = left->z * right->x - right->z * left->x;
    out->z = left->x * right->y - right->x * left->y;
    return out;
}
vec3* vec3_cross(vec3* left, const vec3* right)
{
    vec3 out;
    vec3_cross_copy(&out, left, right);
    return vec3_copy(left, &out);
}

float vec3_dot(const vec3* v1, const vec3* v2)
{
    vec3 out;
    vec3_mult_v3_copy(&out, v1, v2);
    return out.x + out.y + out.z;
}

float vec3_length(const vec3* v)
{
    return mysqrt(vec3_dot(v,v));
}

vec3* vec3_normalize_copy(vec3* out, const vec3* v)
{
    return vec3_mult_v_copy(out, v, 1.f / mysqrt(vec3_dot(v,v)));
}
vec3* vec3_normalize(vec3* v)
{
    vec3 out;
    vec3_normalize_copy(&out, v);
    return vec3_copy(v, &out);
}
vec3* vec3_from_v2(vec3* out, const vec2* v2, float z)
{
    out->x = v2->x;
    out->y = v2->y;
    out->z = z;
    return out;
}


